#include "Sol.hpp"

#define     BDEPOT_INDEX()              (soVRPTW.size())
#define     EDEPOT_INDEX()              (soVRPTW.size() + 1)
#define     SOL_INDEX(VEHICLE, CLIENT)  (((soVRPTW.size() + 2) * (VEHICLE)) + (CLIENT))
#define     SOL_SUCC(VEHICLE, CLIENT)   soSuccs[SOL_INDEX(VEHICLE, CLIENT)]
#define     SOL_PRED(VEHICLE, CLIENT)   soPreds[SOL_INDEX(VEHICLE, CLIENT)]
#define     SOL_INFO(VEHICLE, CLIENT)   soInfos[SOL_INDEX(VEHICLE, CLIENT)]

#define		MAX(A, B)					(((A) > (B)) ? (A) : (B))
#define		MIN(A, B)					(((A) < (B)) ? (A) : (B))


bool 	operator<(const Sol::Eval& in1, const Sol::Eval& in2)
{
	if (in1.seVehicles == in2.seVehicles)
		return (in1.seDistance < in2.seDistance);
		
	return (in1.seVehicles < in2.seVehicles);
}

bool    operator==(const Vehicle::Iterator& in1, const Vehicle::Iterator& in2)
{
    assert(in1.veitVehicle == in2.veitVehicle);
    return (in1.veitValue == in2.veitValue);
}

bool    operator!=(const Vehicle::Iterator& in1, const Vehicle::Iterator& in2)
{
    assert(in1.veitVehicle == in2.veitVehicle);
    return (in1.veitValue != in2.veitValue);
}


Sol::Sol(VRPTW& inVRPTW)
    : soVRPTW(inVRPTW)
{
    try
    {
        soSuccs = new unsigned char[(inVRPTW.size() * (inVRPTW.size() + 2))];
        soPreds = new unsigned char[(inVRPTW.size() * (inVRPTW.size() + 2))];
        soInfos = new ServiceInfo[(inVRPTW.size() * (inVRPTW.size() + 2))];
        soVehicles = new Vehicle[(inVRPTW.size())];
        soTourneeByClient = new unsigned char[inVRPTW.size()];
        soSizeByTournee = new unsigned char[inVRPTW.size()];
        
        memset(soSizeByTournee, 0, inVRPTW.size());
        memset(soTourneeByClient, 0xFF, inVRPTW.size());
        
        for (unsigned i = 0 ; i < inVRPTW.size() ; i++)
        {
			soVehicles[i].reset(&inVRPTW, i);
            soVehicles[i].veId = i;
            soVehicles[i].veSuccs = &soSuccs[(inVRPTW.size() + 2) * i];
            soVehicles[i].vePreds = &soPreds[(inVRPTW.size() + 2) * i];
            soVehicles[i].veInfos = &soInfos[(inVRPTW.size() + 2) * i];
            soVehicles[i].veTourneeByClient= soTourneeByClient;
            soVehicles[i].veSize = &soSizeByTournee[i];
            
            for (unsigned j = 0 ; j < (inVRPTW.size() + 2) ; j++)
                SOL_INFO(i, j).siClient = j;
        }
        
        for (unsigned i = 0 ; i < inVRPTW.size() ; i++)
        {
            SOL_SUCC(i, BDEPOT_INDEX()) = EDEPOT_INDEX();
            SOL_PRED(i, EDEPOT_INDEX()) = BDEPOT_INDEX();
            SOL_PRED(i, BDEPOT_INDEX()) = 0xFE;
            SOL_SUCC(i, EDEPOT_INDEX()) = 0xFE;
            
            ServiceInfo&    sinfo = SOL_INFO(i, BDEPOT_INDEX());
            
            sinfo.siClient = BDEPOT_INDEX();
            sinfo.siArrivalDate = 0.;
            sinfo.siDepartureDate = 0.;
            sinfo.siCCapacityAtArrival = 0;
            sinfo.siCCapacityAtDeparture = 0;
            sinfo.siCDistanceAtArrival = 0.;
            sinfo.siCDistanceAtDeparture = 0.;
            sinfo.siCTimeAtArrival = 0.;
            sinfo.siCTimeAtReady = 0.;
            sinfo.siStayTime = 0.;
            sinfo.siAcceptableLocalDelay = 0.;
            sinfo.siAcceptableGlobalDelay = 0.;
            sinfo.siWaitTime = 0.;
            
            ServiceInfo&    sinfo2 = SOL_INFO(i, EDEPOT_INDEX());
            
            sinfo2.siClient = EDEPOT_INDEX();
            sinfo2.siArrivalDate = 0.;
            sinfo2.siDepartureDate = 0.;
            sinfo2.siCCapacityAtArrival = 0;
            sinfo2.siCCapacityAtDeparture = 0;
            sinfo2.siCDistanceAtArrival = 0.;
            sinfo2.siCDistanceAtDeparture = 0.;
            sinfo2.siCTimeAtArrival = 0.;
            sinfo2.siCTimeAtReady = 0.;
            sinfo2.siStayTime = 0.;
            sinfo2.siAcceptableLocalDelay = soVRPTW.getDepot().deDueDate;
            sinfo2.siAcceptableGlobalDelay = soVRPTW.getDepot().deDueDate;
            sinfo2.siWaitTime = 0.;
        }
    }
    catch(std::bad_alloc&)
    {   assert(false);  }
}

Sol::Sol(const Sol& inSol)
    : soVRPTW(inSol.soVRPTW)
{
    try
    {
        soSuccs = new unsigned char[(soVRPTW.size() * (soVRPTW.size() + 2))];
        soPreds = new unsigned char[(soVRPTW.size() * (soVRPTW.size() + 2))];
        soInfos = new ServiceInfo[(soVRPTW.size() * (soVRPTW.size() + 2))];
        soVehicles = new Vehicle[(soVRPTW.size())];
        soTourneeByClient = new unsigned char[soVRPTW.size()];
        soSizeByTournee = new unsigned char[soVRPTW.size()];
        
        memset(soSizeByTournee, 0, soVRPTW.size());
        memset(soTourneeByClient, 0xFF, soVRPTW.size());
        this->operator=(inSol);
    }
    catch(std::bad_alloc&)
    {   assert(false);  }
}

Sol&    Sol::operator=(const Sol& inSol)
{
    memcpy(soSuccs, inSol.soSuccs, (soVRPTW.size() * (soVRPTW.size() + 2)) * sizeof(unsigned char));
    memcpy(soPreds, inSol.soPreds, (soVRPTW.size() * (soVRPTW.size() + 2)) * sizeof(unsigned char));
    memcpy(soInfos, inSol.soInfos, (soVRPTW.size() * (soVRPTW.size() + 2)) * sizeof(ServiceInfo));
    memcpy(soTourneeByClient, inSol.soTourneeByClient, (soVRPTW.size()) * sizeof(unsigned char));
    memcpy(soSizeByTournee, inSol.soSizeByTournee, (soVRPTW.size()) * sizeof(unsigned char));
    
	for (unsigned i = 0 ; i < soVRPTW.size() ; i++)
	{
		soVehicles[i].veId = i;
		soVehicles[i].veSuccs = &soSuccs[(soVRPTW.size() + 2) * i];
		soVehicles[i].vePreds = &soPreds[(soVRPTW.size() + 2) * i];
		soVehicles[i].veInfos = &soInfos[(soVRPTW.size() + 2) * i];
		soVehicles[i].veTourneeByClient= soTourneeByClient;
		soVehicles[i].veSize = &soSizeByTournee[i];
	}
    
    return (*this);
}

Sol::~Sol()
{
    delete [] soSuccs;
    delete [] soPreds;
    delete [] soInfos;
    delete [] soSizeByTournee;
    delete [] soTourneeByClient;
    delete [] soVehicles;
}

// insert between
void		Vehicle::insert(unsigned char inClient, 
							unsigned char inA,
							unsigned char inB)
{	
	// check
	assert(this->contains(inA));
	assert(this->contains(inB));
	assert(!this->contains(inClient));
		
	// insert
	veSuccs[inA] = inClient;
	veSuccs[inClient] = inB;
	vePreds[inClient] = inA;
	vePreds[inB] = inClient;
	veTourneeByClient[inClient] = veId;
	memset(&veInfos[inClient], 0, sizeof(ServiceInfo));
	veInfos[inClient].siClient = inClient;
	
    this->update(inA);
}

bool		Vehicle::insert2(	unsigned char inClient, 
								unsigned char inA,
								unsigned char inB)
{	
	// check
	assert(this->contains(inA));
	assert(this->contains(inB));
	assert(!this->contains(inClient));
		
	// insert
	veSuccs[inA] = inClient;
	veSuccs[inClient] = inB;
	vePreds[inClient] = inA;
	vePreds[inB] = inClient;
	veTourneeByClient[inClient] = veId;
	memset(&veInfos[inClient], 0, sizeof(ServiceInfo));
	veInfos[inClient].siClient = inClient;
	
    return (this->update2(inA));
}

void                Vehicle::update(unsigned char inClient)
{
    ServiceInfo		linfo;
    double          due_date;
    double          distance;
    unsigned char   client;
	Vehicle::Iterator itA = iterator();
	itA = inClient;
    
    // update down
    linfo = itA.info();
	++itA;
	
	for (; itA != end() ; ++itA)
	{
		client = itA.info().siClient;
        if ((client == veVRPTW->size()) || (client == (veVRPTW->size() + 1)))
            due_date = veVRPTW->getDepot().deDueDate;
        else
            due_date = veVRPTW->operator[](client).clDueDate;
        
		distance = veVRPTW->getDistanceBetween(linfo.siClient, client);
		itA.info().siArrivalDate = VRPTW::tronc(linfo.siDepartureDate + distance);

		//std::cout << (int)linfo.siClient << "->" << (int)client << std::endl;
		//std::cout << linfo.siDepartureDate << "->" << itA.info().siArrivalDate << std::endl;

		/*if ((itA.info().siClient != veVRPTW->size()) && itA.info().siArrivalDate > due_date)
		{
			//std::cout << (int)linfo.siClient << "->" << (int)client << std::endl;
			//std::cout << "ARRIVAL : " << itA.info().siArrivalDate << std::endl;
			//std::cout << "DUE DATE : " << due_date << std::endl;
			assert(false);
		}*/
		
        itA.info().siCCapacityAtArrival = linfo.siCCapacityAtDeparture;
        itA.info().siCCapacityAtDeparture = itA.info().siCCapacityAtArrival + veVRPTW->operator[](client).clDemand;
        assert(itA.info().siCCapacityAtDeparture <= (int)veVRPTW->getVehicleCapacity());
		itA.info().siCDistanceAtDeparture = (itA.info().siCDistanceAtArrival = VRPTW::tronc(linfo.siCDistanceAtDeparture + distance));
		itA.info().siWaitTime = MAX((VRPTW::tronc(veVRPTW->operator[](client).clReadyTime - itA.info().siArrivalDate)), 0.);
		itA.info().siStayTime = VRPTW::tronc(itA.info().siWaitTime + veVRPTW->operator[](client).clServiceTime);
		itA.info().siDepartureDate = VRPTW::tronc(itA.info().siArrivalDate + itA.info().siStayTime);
		itA.info().siAcceptableLocalDelay = VRPTW::tronc(due_date - itA.info().siArrivalDate);
        //std::cout << (int)itA.info().siClient << " ";
        //std::cout << "DEPARTURE : " << itA.info().siDepartureDate << std::endl;
		linfo = itA.info();
	}
	
	//return;
	
	// update up
    Vehicle::Iterator it = this->edepot();
    
    it.info().siAcceptableGlobalDelay = it.info().siAcceptableLocalDelay;
    --it;
    
	for (; it != this->bdepot() ; --it)
	{
		it.info().siAcceptableGlobalDelay = MIN(veInfos[veSuccs[it]].siAcceptableGlobalDelay, it.info().siAcceptableLocalDelay);
	}
}

bool               Vehicle::update2(unsigned char inClient)
{
    ServiceInfo		linfo;
    double          due_date;
    double          distance;
    unsigned char   client;
	Vehicle::Iterator itA = iterator();
	itA = inClient;
    
    // update down
    linfo = itA.info();
	++itA;
	
	for (; itA != end() ; ++itA)
	{
		client = itA.info().siClient;
        if ((client == veVRPTW->size()) || (client == (veVRPTW->size() + 1)))
            due_date = veVRPTW->getDepot().deDueDate;
        else
            due_date = veVRPTW->operator[](client).clDueDate;
        
		distance = veVRPTW->getDistanceBetween(linfo.siClient, client);
		itA.info().siArrivalDate = VRPTW::tronc(linfo.siDepartureDate + distance);
		if (itA.info().siArrivalDate > due_date)
			return (false);
        itA.info().siCCapacityAtArrival = linfo.siCCapacityAtDeparture;
        itA.info().siCCapacityAtDeparture = itA.info().siCCapacityAtArrival + veVRPTW->operator[](client).clDemand;
		if (itA.info().siCCapacityAtDeparture > (int)veVRPTW->getVehicleCapacity())
			return (false);
		itA.info().siCDistanceAtDeparture = (itA.info().siCDistanceAtArrival = VRPTW::tronc(linfo.siCDistanceAtDeparture + distance));
		itA.info().siWaitTime = MAX((VRPTW::tronc(veVRPTW->operator[](client).clReadyTime - itA.info().siArrivalDate)), 0.);
		itA.info().siStayTime = VRPTW::tronc(itA.info().siWaitTime + veVRPTW->operator[](client).clServiceTime);
		itA.info().siDepartureDate = VRPTW::tronc(itA.info().siArrivalDate + itA.info().siStayTime);
		itA.info().siAcceptableLocalDelay = VRPTW::tronc(due_date - itA.info().siArrivalDate);
		linfo = itA.info();
	}
	
	return (true);
	
	// update up
    Vehicle::Iterator it = this->edepot();
    
    it.info().siAcceptableGlobalDelay = it.info().siAcceptableLocalDelay;
    --it;
    
	for (; it != this->bdepot() ; --it)
	{
		it.info().siAcceptableGlobalDelay = MIN(veInfos[veSuccs[it]].siAcceptableGlobalDelay, it.info().siAcceptableLocalDelay);
	}
}


void                Vehicle::erase(unsigned char inClient)
{   
    unsigned char   pred;
    unsigned char	succ;
     
    assert(this->contains(inClient));
    
    // link
    pred = vePreds[inClient];
    succ = veSuccs[inClient];
    veSuccs[vePreds[inClient]] = veSuccs[inClient];
    vePreds[veSuccs[inClient]] = vePreds[inClient];
    // erase
    vePreds[inClient] = 0xFF;
    veSuccs[inClient] = 0xFF;
    memset(&veInfos[inClient], 0, sizeof(ServiceInfo));
    veTourneeByClient[inClient] = 0xFF;
    
    if ((pred == veVRPTW->size()) && (succ == (veVRPTW->size() + 1)))
    {
		//printf("coucou\n");
		this->reset(veVRPTW, veId);
    }
    else
		this->update(pred);
}

std::string			 Vehicle::toString() const
{
	Vehicle::Iterator it;
	std::string		str;
	char			buffer[0x1000];
	
	sprintf(buffer, "[Vehicle %.2u]\t", veId);
	str += buffer;
	
	for (it = this->bdepot() ; it != this->end() ; ++it)
	{
		if (it == this->bdepot())
			sprintf(buffer, "BD -> ");
		else if (it == this->edepot())
			sprintf(buffer, "ED");
		else
			sprintf(buffer, "%2u (%2.2f [W%2.2f:S%2.2f:G%2.2f:L%2.2f] %3u) -> ", 
								it.info().siClient, 
								it.info().siArrivalDate,
								it.info().siWaitTime,
								it.info().siStayTime,
								it.info().siAcceptableGlobalDelay,
								it.info().siAcceptableLocalDelay,
								it.info().siCCapacityAtDeparture);
			
		str += buffer;
	}
	
	return (str);
}


std::string		Sol::toString() const
{
    Sol::Eval   eval;
	std::string str;
	char		buffer[0x1000];
	
    str += "Solution @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@\n";
    
	for (unsigned i = 0 ; i < soVRPTW.size() ; i++)
	{
        if (soVehicles[i].edepot().info().siCDistanceAtArrival == 0.)
            continue;
        
		str += soVehicles[i].toString();
		
		sprintf(buffer, " [%2.2f, %2.2f]", 
					soVehicles[i].edepot().info().siCDistanceAtArrival,
					soVehicles[i].edepot().info().siAcceptableGlobalDelay);
		str += buffer;
		if (i < (soVRPTW.size() - 1))
			str += "\n";
	}
    
    this->eval(eval);
    str += "\n";
    str += "RESULT : ";
    str += eval.toString();
    str += "\n@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@\n";
	
	return str;
}


void            Sol::eval(Sol::Eval& inSolEval) const
{
    std::map<unsigned, double>              vehicles;
    std::map<unsigned, double>::iterator    it;
    
    inSolEval.seComplete = true;
    
    for (unsigned i = 0 ; i < soVRPTW.size() ; i++)
    {
        if (soTourneeByClient[i] == 0xFF)
            inSolEval.seComplete = false;
        else if (vehicles.find(soTourneeByClient[i]) == vehicles.end())
            vehicles[soTourneeByClient[i]] = SOL_INFO(soTourneeByClient[i], (soVRPTW.size() + 1)).siCDistanceAtArrival;
    }
    
    inSolEval.seVehicles = vehicles.size();
    inSolEval.seDistance = 0.;
    
    for (it = vehicles.begin() ; it != vehicles.end() ; ++it)
        inSolEval.seDistance += it->second;
}

void			Sol::clear()
{
	memset(soTourneeByClient, 0xFF, soVRPTW.size() * sizeof(unsigned char));
	memset(soSizeByTournee, 0, soVRPTW.size() * sizeof(unsigned char));
	memset(soSuccs, 0, soVRPTW.size() * (soVRPTW.size() + 2) * sizeof(unsigned char));
	memset(soPreds, 0, soVRPTW.size() * (soVRPTW.size() + 2) * sizeof(unsigned char));
	memset(soInfos, 0, soVRPTW.size() * (soVRPTW.size() + 2) * sizeof(ServiceInfo));
	
	for (unsigned i = 0 ; i < soVRPTW.size() ; i++)
	{
		soVehicles[i].reset(&soVRPTW, i);
		soVehicles[i].veId = i;
		soVehicles[i].veSuccs = &soSuccs[(soVRPTW.size() + 2) * i];
		soVehicles[i].vePreds = &soPreds[(soVRPTW.size() + 2) * i];
		soVehicles[i].veInfos = &soInfos[(soVRPTW.size() + 2) * i];
		soVehicles[i].veTourneeByClient= soTourneeByClient;
		soVehicles[i].veSize = &soSizeByTournee[i];
		
		for (unsigned j = 0 ; j < (soVRPTW.size() + 2) ; j++)
			SOL_INFO(i, j).siClient = j;
	}
	
	for (unsigned i = 0 ; i < soVRPTW.size() ; i++)
	{
		SOL_SUCC(i, BDEPOT_INDEX()) = EDEPOT_INDEX();
		SOL_PRED(i, EDEPOT_INDEX()) = BDEPOT_INDEX();
		SOL_PRED(i, BDEPOT_INDEX()) = 0xFE;
		SOL_SUCC(i, EDEPOT_INDEX()) = 0xFE;
		
		ServiceInfo&    sinfo = SOL_INFO(i, BDEPOT_INDEX());
		
		sinfo.siClient = BDEPOT_INDEX();
		sinfo.siArrivalDate = 0.;
		sinfo.siDepartureDate = 0.;
		sinfo.siCCapacityAtArrival = 0;
		sinfo.siCCapacityAtDeparture = 0;
		sinfo.siCDistanceAtArrival = 0.;
		sinfo.siCDistanceAtDeparture = 0.;
		sinfo.siCTimeAtArrival = 0.;
		sinfo.siCTimeAtReady = 0.;
		sinfo.siStayTime = 0.;
		sinfo.siAcceptableLocalDelay = 0.;
		sinfo.siAcceptableGlobalDelay = 0.;
		sinfo.siWaitTime = 0.;
		
		ServiceInfo&    sinfo2 = SOL_INFO(i, EDEPOT_INDEX());
		
		sinfo2.siClient = EDEPOT_INDEX();
		sinfo2.siArrivalDate = 0.;
		sinfo2.siDepartureDate = 0.;
		sinfo2.siCCapacityAtArrival = 0;
		sinfo2.siCCapacityAtDeparture = 0;
		sinfo2.siCDistanceAtArrival = 0.;
		sinfo2.siCDistanceAtDeparture = 0.;
		sinfo2.siCTimeAtArrival = 0.;
		sinfo2.siCTimeAtReady = 0.;
		sinfo2.siStayTime = 0.;
		sinfo2.siAcceptableLocalDelay = soVRPTW.getDepot().deDueDate;
		sinfo2.siAcceptableGlobalDelay = soVRPTW.getDepot().deDueDate;
		sinfo2.siWaitTime = 0.;
	}
}







